/**
 * @file cont_ff_plan_solver.cpp
 * @brief Implementation of the contingent ff plan solver
 * @date 2024-2-26
*/

#include <sys/stat.h>
#include <sys/types.h>

#include <filesystem>
#include <string>
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <array>
#include <map>

#include "cont_ff_plan_solver/cont_ff_plan_solver.hpp"

namespace ContBTSys {
    // maintain a map of the plan actions and their corresponding locations
    typedef struct _ActionLocation
    {
        std::array<int, 2> cur_location;
        std::string action_name;
        bool is_actuation;
        std::array<int, 2> son_location;
        std::array<int, 2> true_son_location;
        std::array<int, 2> false_son_location;

        // initialize the location of the action to be (-1, -1)
        _ActionLocation() {
            cur_location = {-1, -1};
            son_location = {-1, -1};
            true_son_location = {-1, -1};
            false_son_location = {-1, -1};
            action_name = "";
            is_actuation = false;
        }
    }ActionLocation;
    using CurActionLocation = std::array<int, 2>;
    std::map<CurActionLocation, ActionLocation> location_action_map;

    // get the path of the contingent-ff planner
    std::string get_planner_path() {
        std::string planner_path;
        if (std::getenv("CONTINGENT_FF_PLANNER_PATH") != nullptr) {
            planner_path = std::getenv("CONTINGENT_FF_PLANNER_PATH");
        } else {
            planner_path = "/usr/local/bin/cont_ff";
        }
        return planner_path;
    }

    // Construct the above map from a line of the plan file
    void constructvar_location_action_map(const std::string & line) {
        int pre, cur;
        std::string action_name;
        bool is_actuation = false;
        std::array<int, 2> cur_location = {-1, -1};
        std::array<int, 2> son_location = {-1, -1};
        std::array<int, 2> true_son_location = {-1, -1};
        std::array<int, 2> false_son_location = {-1, -1};

        // get the current location
        pre = 0;
        cur = line.find("||", pre);
        cur_location[0] = std::stoi(line.substr(pre, cur - pre));
        pre = cur + 2;
        cur = line.find(" --- ", pre);
        cur_location[1] = std::stoi(line.substr(pre, cur - pre));
        // get the action name
        pre = cur + 5;
        cur = line.find(" --- ", pre);
        action_name = line.substr(pre, cur - pre);

        // if get the "SON: " substr, means the action is an actuation action
        pre = line.find(" SON:", cur);
        if (pre != std::string::npos) {
            is_actuation = true;
            pre = pre + 5;
            cur = line.find("||", pre);
            son_location[0] = std::stoi(line.substr(pre, cur - pre));
            pre = cur + 2;
            // read until the end of line
            son_location[1] = std::stoi(line.substr(pre));
        } else {
            // if get the "TRUESON" substr, means the action is an observation action
            pre = line.find("TRUESON:", cur);
            if (pre != std::string::npos) {
                pre = pre + 8;
                cur = line.find("||", pre);
                true_son_location[0] = std::stoi(line.substr(pre, cur - pre));
                pre = cur + 2;
                cur = line.find(" --- ", pre);
                true_son_location[1] = std::stoi(line.substr(pre, cur - pre));
            }
            // if get the "FALSESON" substr, means the action is an observation action
            pre = line.find("FALSESON:", cur);
            if (pre != std::string::npos) {
                pre = pre + 9;
                cur = line.find("||", pre);
                false_son_location[0] = std::stoi(line.substr(pre, cur - pre));
                pre = cur + 2;
                false_son_location[1] = std::stoi(line.substr(pre));
            }
        }

        // update the action map using the above information
        ActionLocation al;
        al.cur_location = cur_location;
        al.action_name = action_name;
        al.is_actuation = is_actuation;
        al.son_location = son_location;
        al.true_son_location = true_son_location;
        al.false_son_location = false_son_location;

        location_action_map[cur_location] = al;
    }

    // traversal construct a single node
    ActionPtr traversal_construct_from_single_node(
        const std::map<CurActionLocation, ActionLocation> &var_location_action_map, 
        const CurActionLocation &cur_action_location) {
        
        ActionPtr ret = nullptr;

        // if the action is not found in map, then this action is at the end of the plan
        if (var_location_action_map.find(cur_action_location) == var_location_action_map.end()) {
            return ret;
        }

        // get the action, and construct the action node
        ActionLocation al = var_location_action_map.at(cur_action_location);
        if (al.is_actuation) {
            ret = std::make_shared<Action>(al.action_name, ActionType::ACTUATION);
            ret->son = traversal_construct_from_single_node(var_location_action_map, al.son_location);
        } else {
            ret = std::make_shared<Action>(al.action_name, ActionType::OBSERVATION);
            ret->true_son = traversal_construct_from_single_node(var_location_action_map, al.true_son_location);
            ret->false_son = traversal_construct_from_single_node(var_location_action_map, al.false_son_location);
        }

        return ret;
    }

    // Construct a tree from the above map
    RootPtr construct_tree_from_map(const std::map<CurActionLocation, ActionLocation> &var_location_action_map) {
        CurActionLocation root_location = {0, 0};
        return traversal_construct_from_single_node(var_location_action_map, root_location);
    }

    RootPtr ContFFPlanSolver::getPlan(
        const std::string & domain, const std::string & problem,
        const std::string & node_namespace) {
        
        RootPtr ret = nullptr;
          
        if (node_namespace != "") {
            std::filesystem::path tp = std::filesystem::temp_directory_path();
            for (auto p : std::filesystem::path(node_namespace) ) {
                if (p != std::filesystem::current_path().root_directory()) {
                    tp /= p;
                }
            }
            std::filesystem::create_directories(tp);
        }

        // call the contingent-ff planner to get a plan
        std::cout << "Start solving the above problem using Contingent-ff planner." << std::endl;
        system(
            (get_planner_path() +
            " -a 0 -o " + domain + " -f " + problem +
            " > /tmp/" + node_namespace + "/plan").c_str());

        // read the plan from the file 
        std::string line;
        std::ifstream plan_file("/tmp/" + node_namespace + "/plan");
        bool solution = false;

        // convert the plan into a plan tree
        if (plan_file.is_open()) {
            // read the plan from the file, until read "ff: found plan as follows"
            while (getline(plan_file, line)) {
                if (!solution) {
                    if (line.find("ff: found plan as follows") != std::string::npos) {
                        solution = true;
                    }
                } else if (line.find("||") != std::string::npos) {
                    constructvar_location_action_map(line);
                } else if (line.find("tree layers:") != std::string::npos) {
                    // read at the end of the plan solution
                    break;
                }
            }
            plan_file.close();
        }

        if (!solution) {
            std::cout << "Contingent-ff failed to get a feasible plan." << std::endl;
        }

        // construct the plan tree from the map
        ret = construct_tree_from_map(location_action_map);
        return ret;
    }
}